#include "KalmanTank.h"

KalmanTank::KalmanTank(otank_t* enemy)
{
	this->enemy = enemy;

	//cout << enemy->callsign << endl;

	M = TNT::Array2D<float>(6, 1, 0.0f);
	M[0][0] = enemy->pos[0];
	M[0][3] = enemy->pos[1];


	I = TNT::Array2D<float>(6, 6, 0.0f);

	Ex = TNT::Array2D<float>(6, 6, 0.0f);
	Ez = TNT::Array2D<float>(2, 2, 0.0f);
	Et = TNT::Array2D<float>(6, 6, 0.0f);

	H = TNT::Array2D<float>(2, 6, 0.0f);

	F = TNT::Array2D<float>(6, 6, 0.0f);

	//building the constant matricies
	for(int i = 0; i < I.dim1(); i ++)
		I[i][i] = 1.0f;

	Ez[0][0] = 25.0f;
	Ez[1][1] = 25.0f;

	Ex[0][0] = POSITION;
	Ex[1][1] = VELOCITY;
	Ex[2][2] = ACCELERATION;
	Ex[3][3] = POSITION;
	Ex[4][4] = VELOCITY;
	Ex[5][5] = ACCELERATION;

	
	H[0][0] = 1.0f;
	H[1][3] = 1.0f;

	Ht = TNT::transpose(H);

	for(int i = 0; i < F.dim1(); i ++)
		F[i][i] = 1.0f;

	F[0][1] = TIME_STEP;
	F[0][2] = pow(TIME_STEP,2)/2.0f;
	F[1][2] = TIME_STEP;
	F[2][1] = -FRICTION;
	F[3][4] = TIME_STEP;
	F[3][5] = pow(TIME_STEP,2)/2.0f;
	F[4][5] = TIME_STEP;
	F[5][4] = -FRICTION;

	Ft = TNT::transpose(F);

	//initializing other matricies
	Et[0][0] = 25.0f;
	Et[1][1] = 1.0f;
	Et[2][2] = 1.0f;
	Et[3][3] = 25.0f;
	Et[4][4] = 1.0f;
	Et[5][5] = 1.0f;
}


KalmanTank::~KalmanTank(void)
{
}

void KalmanTank::UpdateMatricies(){
	if(enemy->status.compare("dead")==0)
		return;
	
	TNT::Array2D<float> common = CalcCommon();
	TNT::Array2D<float> observed = TNT::Array2D<float>(2, 1, 0.0f);
	observed[0][0] = enemy->pos[0];
	observed[0][1] = enemy->pos[1];

	K = common*Ht*TNT::invert(H*common*Ht+Ez);
	TNT::Array2D<float> TM = F*M + K*(observed-H*F*M);
	TNT::Array2D<float> TEt = (I-K*H)*common;

	for(int i = 0; i < 6; i ++){
		M[0][i] = TM[0][i];
	}

	for(int i = 0; i < TEt.dim1(); i ++){
		for(int j = 0; j < TEt.dim2(); j ++){
			Et[i][j] = TEt[i][j];
		}
	}
}

Vector2 KalmanTank::Predict(float deltaTime){
	TNT::Array2D<float> PF(6, 6, 0.0f);
	for(int i = 0; i < PF.dim1(); i ++){
		PF[i][i] = 1.0f;
	}
	PF[0][1] = deltaTime;
	PF[0][2] = pow(deltaTime,2)/2.0f;
	PF[1][2] = deltaTime;
	PF[1][2] = -FRICTION;
	PF[3][4] = deltaTime;
	PF[3][5] = pow(deltaTime,2)/2.0f;
	PF[4][5] = deltaTime;
	PF[5][4] = -FRICTION;

	TNT::Array2D<float> TM = PF * M;

	return Vector2(TM[0][0], TM[0][3]);
}

Vector2 KalmanTank::GetCurrentEstimate(){
	return Vector2(M[0][0], M[0][3]);
}

float KalmanTank::Sigma_X(){
	return sqrt(Et[0][0]);
}
float KalmanTank::Sigma_Y(){
	return sqrt(Et[3][3]);
}
float KalmanTank::Rho(){
	return Et[0][3]/(Sigma_X()*Sigma_Y());
}

void KalmanTank::ResetEx(){
	//Et = TNT::Array2D<float>(6, 6, 0.0f);
	for(int i = 0; i < Et.dim1(); i ++)
		for(int j = 0; j < Et.dim2(); j ++)
			Et[i][j] = 0.0f;

	Et[0][0] = 25.0f;
	Et[1][1] = 1.0f;
	Et[2][2] = 1.0f;
	Et[3][3] = 25.0f;
	Et[4][4] = 1.0f;
	Et[5][5] = 1.0f;

	M[0][0] = enemy->pos[0];
	M[0][1] = 0.0f;
	M[0][2] = 0.0f;
	M[0][3] = enemy->pos[1];
	M[0][4] = 0.0f;
	M[0][5] = 0.0f;
	
}

TNT::Array2D<float> KalmanTank::CalcCommon(){
	return F*Et*Ft + Ex;
}